#!/usr/bin/env python3

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node

def generate_launch_description():
    """Generate launch description for ego-planner adapter components only"""
    
    # Get package directory
    ego_adapter_dir = get_package_share_directory('ego_planner_adapter')
    
    # Parameters
    use_sim_time = LaunchConfiguration('use_sim_time')
    namespace = LaunchConfiguration('namespace')
    
    # Declare launch arguments
    declare_use_sim_time = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation time'
    )
    
    declare_namespace = DeclareLaunchArgument(
        'namespace',
        default_value='',
        description='Top-level namespace'
    )
    
    # Configuration files
    trajectory_tracker_params = PathJoinSubstitution([
        ego_adapter_dir, 'config', 'trajectory_tracker_params.yaml'
    ])
    
    target_relay_params = PathJoinSubstitution([
        ego_adapter_dir, 'config', 'target_relay_params.yaml'
    ])
    
    # Trajectory tracker node
    trajectory_tracker_node = Node(
        package='ego_planner_adapter',
        executable='trajectory_tracker_node',
        name='trajectory_tracker',
        namespace=namespace,
        parameters=[
            {'use_sim_time': use_sim_time},
            trajectory_tracker_params
        ],
        remappings=[
            ('odometry', 'odometry'),
            ('trajectory', 'planning/trajectory'),
            ('cmd_vel', 'cmd_vel_nav2_result'),
            ('target_altitude', 'target_altitude'),
            ('trajectory_marker', 'planning/trajectory_marker')
        ],
        output='screen'
    )
    
    # Target relay node
    target_relay_node = Node(
        package='ego_planner_adapter',
        executable='target_relay_node',
        name='target_relay',
        namespace=namespace,
        parameters=[
            {'use_sim_time': use_sim_time},
            target_relay_params
        ],
        remappings=[
            ('goal_pose', 'goal_pose'),
            ('target_altitude', 'target_altitude'),
            ('waypoint_generator/waypoints', 'planning/waypoints'),
            ('planning_trigger', 'planning/trigger'),
            ('goal_marker', 'planning/goal_marker')
        ],
        output='screen'
    )
    
    return LaunchDescription([
        declare_use_sim_time,
        declare_namespace,
        trajectory_tracker_node,
        target_relay_node
    ])